
//// 四关节定义（对应定时器通道）
// #define BASE_JOINT TIM_CHANNEL_1      // 基座旋转（TIM8_CH1）
// #define BIG_ARM_JOINT TIM_CHANNEL_2   // 大臂摆动（TIM8_CH2）
// #define SMALL_ARM_JOINT TIM_CHANNEL_3 // 小臂摆动（TIM8_CH3）
// #define CLAW_JOINT TIM_CHANNEL_4      // 夹爪开合（TIM8_CH4）

#include "robot_arm.h"
#include "tim.h"
#include "main.h"
#include "cmsis_os.h"
#include "ble_remote.h"

extern joint_angle joint;
extern remote_t remote;

// 机械臂初始化
void RobotArm_Init(void);
void RobotArm_Drive(void);

// 各关节角度控制

void RobotArm_BaseDrive(void);
void RobotArm_SetBigArmAngle(float angle);   // 大臂摆动角度（30-150°）
void RobotArm_SetSmallArmAngle(float angle); // 小臂摆动角度（20-160°）
void RobotArm_SetClawAngle(float angle);     // 夹爪开合角度（20-80°）

// 动作控制
void RobotArm_Reset(void);         // 复位到初始位置
void RobotArm_GrabAction(void);    // 抓取动作示例
void RobotArm_ReleaseAction(void); // 释放动作示例

int32_t g_last_angle;

/**
 * @brief 角度转PWM脉冲值
 * @param angle: 目标角度（-90 ~ 90°）
 * @return 脉冲值（50-250）
 */
float Angle_To_Pulse(float angle)
{
    float pulse;
    pulse = (float)PWM_START_PULSE + ((float)(PWM_MAX_PULSE - PWM_START_PULSE) * angle / 90.0f);
    return pulse;
}

static int Direction_Cal(Arm_TypeDef arm)
{
    if (arm == BIG_ARM)
    {
        if (remote.Switch[0] == 0)
        {
            return Stay;
        }
        if (remote.rocker[0].y_position > 0)
        {
            if (joint.BIG_ARM_JOINT >= BIG_ARM_MIN && joint.BIG_ARM_JOINT <= BIG_ARM_MAX)
            {
                if (BIG_ARM_MAX - joint.BIG_ARM_JOINT >= 2)
                {
                    return Forward;
                }
            }

            else
                return Stay;
        }
        else if (remote.rocker[0].y_position < 0)
        {
            if (joint.BIG_ARM_JOINT >= BIG_ARM_MIN && joint.BIG_ARM_JOINT <= BIG_ARM_MAX)
            {
                if (joint.BIG_ARM_JOINT - BIG_ARM_MIN >= 2)
                {
                    return Reverse;
                }
            }

            else
                return Stay;
        }
        else
            return Stay;
    }
    if (arm == SMALL_ARM)
    {
        if (remote.Switch[0] == 0)
        {
            return Stay;
        }
        if (remote.rocker[1].y_position > 0)
        {
            // if ((remote.rocker[1].angle - 0) <= 10)
            if (joint.SMALL_ARM_JOINT >= SMALL_ARM_MIN && joint.SMALL_ARM_JOINT <= SMALL_ARM_MAX)
            {
                if (joint.SMALL_ARM_JOINT - SMALL_ARM_MIN >= 2)
                {
                    return Reverse;
                }
            }

            else
                return Stay;
        }
        else if (remote.rocker[1].y_position < 0)
        {
            // if ((remote.rocker[1].angle - 180) <= 10)
            if (joint.SMALL_ARM_JOINT >= SMALL_ARM_MIN && joint.SMALL_ARM_JOINT <= SMALL_ARM_MAX)
            {
                if (SMALL_ARM_MAX - joint.SMALL_ARM_JOINT >= 2)
                {
                    return Forward;
                }
            }
            else
                return Stay;
        }
        else
            return Stay;
    }
}

void Smooth_Move(int32_t direction, Arm_TypeDef arm)
{

    if (arm == BIG_ARM)
    {
        if (remote.Switch[0] == 0)
        {
            return;
        }
        if (remote.rocker[0].distance <= DEAD_ZONE)
            return;
        if (remote.rocker[0].distance <= 300)
        {
            joint.BIG_ARM_JOINT += direction * 2;
            osDelay(120);
        }
        else if (remote.rocker[0].distance > 300)
        {
            joint.BIG_ARM_JOINT += direction * 2;
            osDelay(60);
        }
    }
    else if (arm == SMALL_ARM)
    {
        if (remote.Switch[0] == 0)
        {
            return;
        }
        if (remote.rocker[1].distance <= DEAD_ZONE)
            return;
        if (remote.rocker[1].distance <= 300)
        {
            joint.SMALL_ARM_JOINT += direction * 2;
            osDelay(120);
        }
        else if (remote.rocker[1].distance > 300)
        {
            joint.SMALL_ARM_JOINT += direction * 2;
            osDelay(60);
        }
    }
    else if (arm == CLAW_ARM)
    {
        if (remote.Switch[0] == 0)
            return;
        if (remote.Button[3] == 1 && remote.Button[2]==0)
        {
            joint.CLAW_JOINT += 2;
            osDelay(40);
        }
        else if (remote.Button[2] == 1 && remote.Button[3] == 0)
        {
            joint.CLAW_JOINT -= 2;
            osDelay(20);
        }
    }
}

void Servo_Protect()
{

    if (joint.BIG_ARM_JOINT <= BIG_ARM_MIN)
        joint.BIG_ARM_JOINT = BIG_ARM_MIN + 1;
    if (joint.BIG_ARM_JOINT >= BIG_ARM_MAX)
        joint.BIG_ARM_JOINT = BIG_ARM_MAX - 1;

    if (joint.SMALL_ARM_JOINT <= SMALL_ARM_MIN)
        joint.SMALL_ARM_JOINT = SMALL_ARM_MIN + 1;
    if (joint.SMALL_ARM_JOINT >= SMALL_ARM_MAX)
        joint.SMALL_ARM_JOINT = SMALL_ARM_MAX - 1;
    if (joint.CLAW_JOINT <= CLAW_MIN)
        joint.CLAW_JOINT = CLAW_MIN + 1;
    if (joint.CLAW_JOINT >= CLAW_MAX)
        joint.CLAW_JOINT = CLAW_MAX - 1;
}

void Update_Servo_PWM()
{
    Servo_Protect();
    __HAL_TIM_SetCompare(&htim8, BIG_ARM_JOINT_CH, Angle_To_Pulse(joint.BIG_ARM_JOINT));
    __HAL_TIM_SetCompare(&htim8, SMALL_ARM_JOINT_CH, Angle_To_Pulse(joint.SMALL_ARM_JOINT));
    __HAL_TIM_SetCompare(&htim8, CLAW_JOINT_CH, Angle_To_Pulse(joint.CLAW_JOINT));
}

/**
 * @brief 初始化机械臂（启动PWM输出）
 */

void RobotArm_Init(void)
{
    // 启动所有关节的PWM输出
    HAL_TIM_PWM_Start(&htim8, BASE_JOINT_CH);
    HAL_TIM_PWM_Start(&htim8, BIG_ARM_JOINT_CH);
    HAL_TIM_PWM_Start(&htim8, SMALL_ARM_JOINT_CH);
    HAL_TIM_PWM_Start(&htim8, CLAW_JOINT_CH);

    // 复位到初始位置
    RobotArm_Reset();
}

void RobotArm_BaseDrive()
{
    if (remote.Button[0] == 1 && remote.Button[1] == 0)
    {
        __HAL_TIM_SetCompare(&htim8, BASE_JOINT_CH, TurnLeft);
    }
    else if (remote.Button[1] == 1 && remote.Button[0] == 0)
    {
        __HAL_TIM_SetCompare(&htim8, BASE_JOINT_CH, TurnRight);
    }
    else
        __HAL_TIM_SetCompare(&htim8, BASE_JOINT_CH, 150);
}

void Claw_GrabeDrive()
{
    if (remote.Switch[0] == 0)
        return;
    int32_t C_direction;
    Smooth_Move(C_direction, CLAW_ARM);
}

/**
 * @brief 舵机最终驱动函数
 * @param
 * @return
 */
void RobotArm_Drive()
{
    RobotArm_BaseDrive();
    Claw_GrabeDrive();
    int32_t B_direction, S_direction;
    B_direction = Direction_Cal(BIG_ARM);
    S_direction = Direction_Cal(SMALL_ARM);
    Smooth_Move(B_direction, BIG_ARM);
    Smooth_Move(S_direction, SMALL_ARM);
    Update_Servo_PWM();
}

/**
 * @brief 设置大臂摆动角度(调试用)
 * @param angle: 目标角度（30-150°）
 */
void RobotArm_SetBigArmAngle(float angle)
{

    __HAL_TIM_SET_COMPARE(&htim8, BIG_ARM_JOINT_CH, Angle_To_Pulse(angle));
}

/**
 * @brief 设置小臂摆动角度
 * @param angle: 目标角度（20-160°）
 */
void RobotArm_SetSmallArmAngle(float angle)
{

    __HAL_TIM_SET_COMPARE(&htim8, SMALL_ARM_JOINT_CH, Angle_To_Pulse(angle));
}

/**
 * @brief 设置夹爪开合角度
 * @param angle: 目标角度（20°张开，80°闭合）
 */
void RobotArm_SetClawAngle(float angle)
{
    __HAL_TIM_SET_COMPARE(&htim8, CLAW_JOINT_CH, Angle_To_Pulse(angle));
}

/**
 * @brief 机械臂复位（回到初始位置）
 */
void RobotArm_Reset()
{
    //Servo_Protect();
    joint.BIG_ARM_JOINT = -50;  // 大臂抬起
    joint.SMALL_ARM_JOINT = 60; // 小臂收起
    joint.CLAW_JOINT = -90;     // 夹爪张开
    Update_Servo_PWM();
    osDelay(1000); // 等待舵机到位
}